#include "npu_robot/navigation.h"

MoveBaseClient client("move_base", true);

void Navigation::init(void)
{
    // client = MoveBaseClient("move_base", true); // true -> don't need ros::spin()

    navigationState = NAVIGATION_PATROL;

    goalGlobal.init();
    goalGlobal.goalRegister(new GoalClass(0.072, -1.0, 1.0, 0.0));
    goalGlobal.goalRegister(new GoalClass(2.7, -4.5, 0.7, 0.7));
    goalGlobal.goalRegister(new GoalClass(4.5, -4.5, 0.7, 0.7));
    goalGlobal.goalRegister(new GoalClass(5.3, -0.6, 1.0, 0.0));
}

void Navigation::patrol(void)
{
    sendGoal(goalGlobal.getNextGoal(true));
    while (navigationState == NAVIGATION_PATROL)
    {
        sendGoal(goalGlobal.getNextGoal());
    }
}

void Navigation::stop(void)
{
}

void Navigation::sendGoal(GoalClass *goal)
{
    client.waitForServer();
    client.sendGoal(goal->getGoal());
    client.waitForResult(ros::Duration(20.0));
}

Navigation navigation;
